
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       p_1d.c
  * @author     baiyang
  * @date       2021-8-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "p_1d.h"
#include "p.h"
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void p_1d_ctrl_ctor(P_1d_ctrl* controler, float initial_p, float dt)
{
    controler->_dt = dt;
    controler->_kp = initial_p;
}

// update_all - set target and measured inputs to P controller and calculate outputs
// target and measurement are filtered
// if measurement is further than error_min or error_max (see set_limits method)
//   the target is moved closer to the measurement and limit_min or limit_max will be set true
float p_1d_ctrl_update_all(P_1d_ctrl* controler, float *target, float measurement, bool *limit_min, bool *limit_max)
{
    *limit_min = false;
    *limit_max = false;

    // calculate distance _error
    controler->_error = *target - measurement;

    if (math_flt_negative(controler->_error_min) && (controler->_error < controler->_error_min)) {
        controler->_error = controler->_error_min;
        *target = measurement + controler->_error;
        *limit_min = true;
    } else if (math_flt_positive(controler->_error_max) && (controler->_error > controler->_error_max)) {
        controler->_error = controler->_error_max;
        *target = measurement + controler->_error;
        *limit_max = true;
    }

    // MIN(_Dxy_max, _D2xy_max / _kxy_P) limits the max accel to the point where max jerk is exceeded
    return p_ctrl_sqrt_controller(controler->_error, controler->_kp, controler->_D1_max, controler->_dt);
}

// set_limits - sets the maximum error to limit output and first and second derivative of output
// when using for a position controller, lim_err will be position error, lim_out will be correction velocity, lim_D will be acceleration, lim_D2 will be jerk
void p_1d_ctrl_set_limits(P_1d_ctrl* controler, float output_min, float output_max, float D_Out_max, float D2_Out_max)
{
    controler->_D1_max = 0.0f;
    controler->_error_min = 0.0f;
    controler->_error_max = 0.0f;

    if (math_flt_positive(D_Out_max)) {
        controler->_D1_max = D_Out_max;
    }

    if (math_flt_positive(D2_Out_max) && math_flt_positive(controler->_kp)) {
        // limit the first derivative so as not to exceed the second derivative
        controler->_D1_max = MIN(controler->_D1_max, D2_Out_max / controler->_kp);
    }

    if (math_flt_negative(output_min) && math_flt_positive(controler->_kp)) {
        controler->_error_min = inv_sqrt_controller(output_min, controler->_kp, controler->_D1_max);
    }

    if (math_flt_positive(output_max) && math_flt_positive(controler->_kp)) {
        controler->_error_max = inv_sqrt_controller(output_max, controler->_kp, controler->_D1_max);
    }
}

// set_error_limits - reduce maximum error to error_max
// to be called after setting limits
void p_1d_ctrl_set_error_limits(P_1d_ctrl* controler, float error_min, float error_max)
{
    if (math_flt_negative(error_min)) {
        if (!math_flt_zero(controler->_error_min)) {
            controler->_error_min = MAX(controler->_error_min, error_min);
        } else {
            controler->_error_min = error_min;
        }
    }

    if (math_flt_positive(error_max)) {
        if (!math_flt_zero(controler->_error_max)) {
            controler->_error_max = MIN(controler->_error_max, error_max);
        } else {
            controler->_error_max = error_max;
        }
    }
}
/*------------------------------------test------------------------------------*/


